﻿using System;
using System.Diagnostics;
using System.Runtime.InteropServices;
using SharpDX;
using SharpDX.XAPO;
using SynergyLib.DSP;
using SynergyLib.SMath;

namespace XAudio2Controls
{
    /// <summary>
    /// G is the logarithmic gain in dB
    /// Fc is the center frequency
    /// Fs is the sample rate
    /// Q adjusts the slope
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct ShelvingPeakFilterParams
    {
        public float G { get; set; }
        public float Fc { get; set; }  //<1
        public float Q { get; set; }
    }


    public class ShelvingPeakFilter : AudioProcessorBase<ShelvingPeakFilterParams>
    {
        private float d
        {
            get { return (float)-Math.Cos(Trig.TwoPI * Parameters.Fc / InputFormatLocked.SampleRate); }
        }
        private float K
        {
            get { return (float)Math.Tan((Math.PI * d) / InputFormatLocked.SampleRate); }
        }
        private float V0
        {
            get
            {
                return (float)Math.Pow(10, (Parameters.G / 20));
            }
            set
            {
                float temp = value;
                V0 = temp;
            }
        }
        private float root2
        {
            get { return (float)(1 / Parameters.Q); }
        }

        private float aB
        { get { return GetLimsForBoost(); } }
        private float aC
        {
            get { return GetLimsForCut(); }
        }
        private float H0
        {
            get { return V0 - 1; }
        }

        #region Filter Coefficients

        private float b0 { get; set; }
        private float b1 { get; set; }
        private float b2 { get; set; }
        private float a1 { get; set; }
        private float a2 { get; set; }

        #endregion Filter Coefficients

        public override void Process(BufferParameters[] inputProcessParameters, BufferParameters[] outputProcessParameters, bool isEnabled)
        {
            int frameCount = inputProcessParameters[0].ValidFrameCount;
            DataStream input = new DataStream(inputProcessParameters[0].Buffer, frameCount * InputFormatLocked.BlockAlign, true, true);
            DataStream Output = new DataStream(inputProcessParameters[0].Buffer, frameCount * InputFormatLocked.BlockAlign, true, true);


            float[] leftX = new float[frameCount];
            float[] rightX = new float[frameCount];

            //to avoid referencing negative samples
            leftX[0] = input.Read<float>();
            rightX[0] = input.Read<float>();
            leftX[1] = input.Read<float>();
            rightX[1] = input.Read<float>();


            float[] yns = new float[frameCount];
            yns[0] = leftX[0];
            yns[1] = leftX[1];

            for (int n = 2; n < frameCount; n++, Counter++)
            {
                leftX[n] = input.Read<float>();
                rightX[n] = input.Read<float>();

                float top = (aB * leftX[n]) + (d * (1 - aB)) * leftX[n - 1] + leftX[n - 2];
                float bot = (d * (1 - aB)) * leftX[n - 1] + aB * leftX[n - 2];
                float y1n = top - bot;
                yns[n] = ((H0 / 2) * (leftX[n] - y1n)) + leftX[n];


            }
            for (int i = 0; i < frameCount; i++)
            {
                Output.Write(yns[i]);
                Output.Write(yns[i]);
            }
        }

        private float GetLimsForBoost()
        {
            float num = (float)Math.Tan(Trig.TwoPI * Parameters.Q / InputFormatLocked.SampleRate) - 1;
            float den = (float)Math.Tan(Trig.TwoPI * Parameters.Q / InputFormatLocked.SampleRate) + 1;
            return (num / den);
        }
        private float GetLimsForCut()
        {
            float num = (float)Math.Tan(Trig.TwoPI * Parameters.Q / InputFormatLocked.SampleRate) - V0;
            float den = (float)Math.Tan(Trig.TwoPI * Parameters.Q / InputFormatLocked.SampleRate) - V0;
            return (num / den);
        }
    }
}

